//import java.util.regex.PatternSyntaxException;
import lejos.nxt.Button;
import lejos.nxt.LCD;
import lejos.nxt.LightSensor;
import lejos.robotics.subsumption.Arbitrator;
import lejos.robotics.subsumption.Behavior;
import lejos.nxt.Motor;
import lejos.nxt.MotorPort;
import lejos.nxt.SensorPort;
import lejos.nxt.UltrasonicSensor;
import lejos.robotics.TachoMotor;
import lejos.robotics.navigation.TachoPilot;
import lejos.nxt.ButtonListener;

/**
 *
 * @author Nicolas
 */
public class Robot {

    private static final MotorPort PORT_MOTEUR_GAUCHE = MotorPort.A;
    private static final MotorPort PORT_MOTEUR_DROIT = MotorPort.B;
    private static final SensorPort PORT_LIGHT_SENSOR = SensorPort.S2;
    private static final SensorPort PORT_ULTRASONIC_SENSOR = SensorPort.S3;
    private static final float DIAMETRE_ROUE = 4.45f;//5.95f
    private static final float ECARTEMENT = 11.0f;// 13.5f;
    /**
     * Capteur de luminosité
     */
    public static final LightSensor LightSensor = new LightSensor(PORT_LIGHT_SENSOR);
    /**
     * Capteur à ultrason
     */
    public static final UltrasonicSensor UltrasonicSensor = new UltrasonicSensor(PORT_ULTRASONIC_SENSOR);

    ;
    /**
     * Moteur gauche
     */
    public static final Motor MoteurGauche = new Motor(PORT_MOTEUR_GAUCHE);
    /**
     * Moteur droit
     */
    public static final Motor MoteurDroit = new Motor(PORT_MOTEUR_DROIT);
    /**
     * Outil de pilotage du robot
     */
    public static final TachoPilot Pilot = new TachoPilot(DIAMETRE_ROUE, ECARTEMENT, (TachoMotor) MoteurGauche, (TachoMotor) MoteurDroit);
    /**
     * Nombre de cases parcourues
     */
    private static int cases_parcourues = 0;
    /**
     * sens courant du robot
     */
    private static boolean sens_normal = true;
    /**
     * couloir de droite 
     */
    public static Range GRIS_CLAIR = new Range(466,520);
    /** 
     * couloir de gauche
     */
    public static Range GRIS_INTERMEDIAIRE = new Range(445,465);
    /**
     * piste à suivre
     */
    public static Range GRIS_FONCE = new Range(402,444);
    /**
     * seuil de franchissement de cellule
     */
    public static Range NOIR = new Range(0, 401);
    /**
     *  intersection ou changement de direction
     */
    public static Range BLANC = new Range(521, 1000);
    /**
     *  position du capteur de lumière par rapport à l'axe des roues
     */
    public static final float POSITON_LIGHT_SENSOR = 6f;
    /**
     *  position du capteur de lumière par rapport à l'axe des roues
     */
    public static boolean OBSTACLE_DETECTE = false;     
    
    /**
     *  position du capteur de lumière par rapport à l'axe des roues
     */
    public static boolean CHANGEMENT_CELLULE = false;
    /**
     * Distance maximale séparant le robot d'un obstacle
     */
    public static int SEUIL_OBSTACLE = 10;
    public static int MAX_POWER = 10;
    /**
     * Case courante du robot
     */
    public static Case POSITION;

    /**
     * Direction courante de déplacement du robot
     */
    public static Direction DIRECTION;    
    
    
    /**
     *
     */
    public static void initialiser() {
        
        UltrasonicSensor.continuous();
        Pilot.setMoveSpeed(5f);
        Pilot.setTurnSpeed(30f);
        Robot.MoteurDroit.regulateSpeed(false);
        Robot.MoteurGauche.regulateSpeed(false);
        DIRECTION = Direction.NORD;
        POSITION = Labyrinthe.getCaseDepart();        

        try {
            UltrasonicSensor.continuous();
        } catch (Exception ex) {
            Robot.justSay("Erreur robot(), continuous");
            try {
                Thread.sleep(1000);
            } catch (Exception exit) {
            }
        }

        try {
            Button.ESCAPE.addButtonListener(new ButtonListener() {

                public void buttonPressed(Button b) {
                    System.exit(0);
                }

                public void buttonReleased(Button b) {
                }
            });
        } catch (Exception ex) {
            Robot.justSay("Erreur Listener escape");
            try {
                Thread.sleep(1000);
            } catch (Exception exit) {
            }


        }
    }

    /**
     *
     * @param args
     * @throws Exception
     */
    public static void main(String[] args) throws Exception {
	
		//Initialisation du labyrinthe
        Labyrinthe.initialiser(5,7,new Case(1,1),new Case(5,7));
        Robot.initialiser();
        LCD.clear();
        Labyrinthe.draw(0, 0);
        int button_id;
        while (true) {
            afficherMenu();
            button_id = Button.waitForPress();
            switch (button_id) {
                case 1:
                    Button.ENTER.waitForPressAndRelease();
                    autoCalibrer();
                    naviguer();
                    break;//Bouton Enter
                case 2:
                    Button.LEFT.waitForPressAndRelease();
                    calibrerCouleur();
                    break; //Bouton Left
                case 4:
                    Button.RIGHT.waitForPressAndRelease();
                    calibrerSeuilObstacle();
                    break;//Bouton Right
                case 8:
                    Button.ESCAPE.waitForPressAndRelease();
                    System.exit(0);
                    break;//Bouton Escape
                default:
            }
        }

    }

    /**
     * Engage la navigation du robot
     */
    public static void naviguer() {
        LCD.clear();
        LCD.drawString("Navigation !", 0, 0);
        LCD.refresh();
        
        Behavior suivreLigne = new SuivreLigne();//Couleur = gris intermediaire
        Behavior trouverLigneAGauche = new TrouverLigneAGauche();//On est sur du gris clair
        Behavior trouverLigneADroite = new TrouverLigneADroite();//On est sur du gris foncé
        Behavior faireDemiTour = new FaireDemiTour();//obstacle détecté
        Behavior changerDeCellule = new ChangerDeCellule();//couleur détectée= noir
        Behavior changerDeDirection = new ChangerDeDirection();//case blanche
        Behavior tester = new Tester();//case blanche
        Behavior feterArrivee= new FeterArrivee();//le robot est arrivé 
        //Behavior[] bArray = {tester,suivreLigne, trouverLigneAGauche, trouverLigneADroite, changerDeCellule, faireDemiTour, changerDeDirection};
        Behavior[] bArray = {tester,suivreLigne, trouverLigneADroite, faireDemiTour, changerDeDirection, changerDeCellule,FeterArrivee};
        Arbitrator arby = new Arbitrator(bArray);
        arby.start();
    }

    /**
     *  Récupère le nombre de cases parcourues
     * @return le nombre de cases parcourues
     */
    public static int getNombreCasesParcourues() {
        return cases_parcourues;
    }

    /**
     * Augmente le compte des cases parcourues
     * @see ChangerDeCellule.action()
     */
    public static void augmenterNombreCasesParcourues() {
        cases_parcourues++;
    }
   /**
     *  Être en sens normal
     * @return true si le robot est en sens normal
     */
    public static boolean estEnSensNormal() {
        return sens_normal;
    }
    /**
     *  Mémorise s'il l'on est en sens normal ou non
     */
    public static void estEnSensNormal(boolean est_en_sens_normal) {
        sens_normal = est_en_sens_normal;
    }

    /**
     * Mémorise le fait qu'on ait changé de sens
     */
    public static void inverserSensDeNavigation() {
        sens_normal = !sens_normal;
    }
    
    /**
     * @return boolean : vrai si robot est sur du noir
     */
    public static boolean estSurDuNoir() {
        return Robot.NOIR.contient(Robot.readLightValue());
    }
    
    /**
     * @return boolean : vrai si robot est sur du blanc
     */
    public static boolean estSurDuBlanc() {
        return Robot.BLANC.contient(Robot.readLightValue());
    }
    
    /**
     * @return boolean : vrai si robot est sur du noir
     */
    public static void autoCalibrer() {
		
		//on est sur la case noir
        Robot.NOIR.setBorneSuperieur(Robot.readLightValue()+30);
        Robot.Pilot.forward();
        while(Robot.estSurDuNoir()){}
        Robot.Pilot.stop();
        //On avance pour que le faisceau ne soit pas à cheval sur le noir et la case
        Robot.Pilot.travel(6);
		
        Robot.effectuerBalayage();
		
        int valeur_min = 1000;
        int valeur_max = 0;
        int valeur_lue;
        int i =0;
		
        Robot.Pilot.reset();
        Robot.Pilot.steer(-100,45,true);
        while(Robot.Pilot.isMoving()){
            valeur_lue = Robot.readLightValue();
            //mesure[i++] = valeur_lue;
            if(valeur_lue < valeur_min)
                valeur_min = valeur_lue;
            if(valeur_lue > valeur_max)
                valeur_max = valeur_lue;
        }
        LCD.drawString("i :"+i+"               ", 0, 0);
        
        Button.ENTER.waitForPressAndRelease();
        if(valeur_min>Robot.NOIR.getBorneSuperieur())
            Robot.NOIR.setBorneSuperieur(valeur_min+10);

        Robot.Pilot.steer(-100,-45,true);
        while(Robot.Pilot.isMoving()){}
        
        Robot.Pilot.travel(8,true);  
        while(Robot.Pilot.isMoving()){
            valeur_lue = Robot.readLightValue();
            if(valeur_lue < valeur_min)
                valeur_min = valeur_lue;
            if(valeur_lue > valeur_max)
                valeur_max = valeur_lue;
        }
		
        if(valeur_min>Robot.NOIR.getBorneSuperieur())
            Robot.NOIR.setBorneSuperieur(valeur_min+10);
        
        LCD.drawString("NOIR : "+valeur_min,0,0);
        LCD.drawString("BLANC : "+valeur_max,0,1);
        LCD.refresh();
        Button.ENTER.waitForPressAndRelease();

        Robot.BLANC.setBorneInferieur(valeur_max-30);
        Robot.reculerJusquaBordCase();
        LCD.drawString("BORD CASE !"+valeur_min,0,1);
        LCD.refresh();
        //On se place au centre de la case
        Robot.Pilot.travel(-5f);
        Robot.effectuerBalayage();

        LCD.drawString("GRIS "+valeur_max,0,1);
        LCD.refresh();
 
        Robot.Pilot.stop();
        //Retour à la case départ
        Robot.Pilot.travel(-8f);
    }
	
    /**
	 * Effectue un balayage de 120° afin de déterminer les différentes valeurs de couleurs
	 * mesurée par le capeur
	 *
	 **/
    public static void effectuerBalayage(){
        //Et on entame un balayage
        Robot.Pilot.rotate(60);
        Robot.Pilot.stop();
        Robot.Pilot.reset();
        Robot.MoteurDroit.backward();
        Robot.MoteurGauche.forward();
        int i = 0;
        float angle=Math.abs(Robot.Pilot.getAngle());
        //On va effectuer un balayage de 120
        int angle_max = 120;
        int[] mesures = new int[angle_max+1];
        int[] seuils = new int[20];
        int[] tangeante_seuils = new int[20];
        int col=0;
        int row = 0;
        String resultat="";
        boolean affiche = false;
        //Robot.Pilot.steer(100,60,true);
        LCD.clear();
        while(angle<=angle_max){
            //On a franchi un nouveau pas => on enregistre la valeur
            angle = Math.abs(Robot.Pilot.getAngle()); 
            if(angle>=i){
                mesures[i]=Robot.readLightValue();
				//Pour afficher les mesures sur l'ecran
                if(affiche){
                    LCD.drawString(Integer.toString(mesures[i]), col, row);
                    col+=3;
                    if(col%15==0){
                        col=0;
                        row++;
                    }
					
                    if(++i%40==0){
                        LCD.refresh();
                        Robot.Pilot.stop();
                        Button.ENTER.waitForPressAndRelease();
                        col=0;
                        row=0;
                        Robot.MoteurDroit.backward();
                        Robot.MoteurGauche.forward();
                    }
                } else {
                    i++;
                } 
            }
            //Robot.Pilot.rotate(2f);
        }
        int j;
        int nombre_seuils=0;
        Robot.Pilot.stop();
        boolean nouvelle_valeur;
        int tangeante;
        //LCD.clear();
        for(i=1;i<=angle_max-1;i++){
            tangeante = Math.abs(mesures[i-1]-mesures[i+1]);
            if(tangeante<=3){                
                nouvelle_valeur=true;
                if(nombre_seuils>0){
                    for(j=0;j<nombre_seuils;j++){
                        if(Math.abs(seuils[j]-mesures[i])<=10){
                            //tangeante plus petite que celle du seuil de la même couleur
                            //=>on remplace par la nouvelle valeur
                            if(tangeante<tangeante_seuils[j]){
                                seuils[j]=mesures[i];
                            }
                            nouvelle_valeur=false;
                        }
                    }
                }
                if(nouvelle_valeur){
                    try{
                        seuils[nombre_seuils]=mesures[i];
                        tangeante_seuils[nombre_seuils]=Math.abs(mesures[i-1]-mesures[i+1]);
                        nombre_seuils++;
                    } catch(Exception Ex){
                        
                    }
                    
                    resultat+=mesures[i]+",";
                }
                
            }
        }
        int indice_noir=0;
        if(seuils[0]>seuils[1])
            indice_noir=1;
        //on a 5 seuils : noir , gris foncé, gris, gris clair et blanc
		int valeur_noir=0;
		if(seuils[indice_noir+4]<seuils[indice_noir]){
			valeur_noir=seuils[indice_noir+4];
		} else {
			valeur_noir=seuils[indice_noir];
		}
        Robot.NOIR.setBorneSuperieur(valeur_noir+10);
		
        Robot.GRIS_FONCE.setBorneInferieur(valeur_noir+11);
        Robot.GRIS_FONCE.setBorneSuperieur(seuils[indice_noir+2]-11);
		
        Robot.GRIS_INTERMEDIAIRE.setBorneInferieur(seuils[indice_noir+2]-10);
        Robot.GRIS_INTERMEDIAIRE.setBorneSuperieur(seuils[indice_noir+2]+10); 
		
        Robot.GRIS_CLAIR.setBorneInferieur(seuils[indice_noir+2]+11);
        Robot.GRIS_CLAIR.setBorneSuperieur(seuils[indice_noir+3]+10); 
		
		Robot.BLANC.setBorneInferieur(seuils[indice_noir+3]+11);
     
      /*
            for(i=0;i<nombre_seuils;i++){
                LCD.drawString(seuils[i]+",",i%3*4,i/3);
            }
		*/
		//retour sur la ligne
        Robot.MoteurDroit.forward();
        Robot.MoteurGauche.backward();
        while(!Robot.estSurLaLigne()){
            valeur_lue = readLightValue();
        }        
     }
    
    public static void reculerJusquaBordCase(){
        Robot.Pilot.backward();
        while(!Robot.estSurDuNoir()){}
        Robot.Pilot.stop();
    }
    
    public static void avancerJusquaBordCase(){
        Robot.Pilot.forward();
        while(!Robot.estSurDuNoir()){}
        Robot.Pilot.stop();
    }
    /**
     * @return boolean : vrai si robot est sur la ligne
     */
    public static boolean estSurLaLigne() {
        return Robot.GRIS_INTERMEDIAIRE.contient(Robot.readLightValue());
    }

    /**
     * @return boolean : vrai si robot est sur la case d'arrivée
     */
    public static boolean estArrive() {
        return Robot.POSITION.equals(Labyrinthe.getCaseArrivee());
    }

    /**
     * Met à jour la position courante du robot
     * @param rotation : nombre de quarts de tour effectués lors du changement de case
     * Ex : -1 pour quart de tour vers la droite
     *      1 pour quart de tour vers la gauche
     *      2 : pour une demi tour
     */
    public static void mettreAjourDirection(int nombre_quart_de_tour) {
        Robot.DIRECTION.rotate(nombre_quart_de_tour);
        LCD.drawString("n dir :"+Robot.DIRECTION.getLabel(), 0, 5);
        LCD.refresh(); 
    }
    
    public static void mettreAJourPosition() {
        Robot.POSITION.translate(Robot.DIRECTION);
        LCD.drawString(Robot.DIRECTION.getLabel(), 12, 0);
        LCD.drawString("("+Robot.POSITION.getX()+","+Robot.POSITION.getY()+")", 10, 1);
        LCD.refresh(); 
    }
    
    /**
     * Calibrage de la couleur
     * Modifie les valeurs couleur de référence
     */
    public static void calibrerCouleur() {
        try {
            int valeurTestee = 0;
            LCD.clearDisplay();
            LCD.drawString("Valider : ENTER", 0, 2);
            LCD.drawString("noir            ", 0, 0);
            while (!Button.ENTER.isPressed()) {
                valeurTestee = readLightValue();
                LCD.drawString("Valeur :" + valeurTestee + "     ", 0, 1);
                LCD.refresh();
            }
            NOIR = new Range(0, valeurTestee+20);

            Button.ENTER.waitForPressAndRelease();

            LCD.drawString("blanc           ", 0, 0);
            while (!Button.ENTER.isPressed()) {
                valeurTestee = readLightValue();
                LCD.drawString("Valeur :" + valeurTestee + "     ", 0, 1);
                LCD.refresh();
            }
            BLANC = new Range(valeurTestee - 10, 800);

            Button.ENTER.waitForPressAndRelease();

            LCD.drawString("gris intermediaire", 0, 0);
            while (!Button.ENTER.isPressed()) {
                valeurTestee = readLightValue();
                LCD.drawString("Valeur :" + valeurTestee + "     ", 0, 1);
                LCD.refresh();
            }
            GRIS_INTERMEDIAIRE = new Range(valeurTestee - 10, valeurTestee + 10);

            Button.ENTER.waitForPressAndRelease();

            LCD.drawString("gris clair      ", 0, 0);
            while (!Button.ENTER.isPressed()) {
                valeurTestee = readLightValue();
                LCD.drawString("Valeur :" + valeurTestee + "     ", 0, 1);
                LCD.refresh();
            }
            if (GRIS_INTERMEDIAIRE.contient(valeurTestee)) {
                GRIS_INTERMEDIAIRE = new Range(valeurTestee - 25, valeurTestee - 5);
            }
            if (BLANC.contient(valeurTestee)) {
                //On a pris un intervalle trop grand => on corrige
                BLANC = new Range(valeurTestee + 1, 100);
            }
            GRIS_CLAIR = new Range(GRIS_INTERMEDIAIRE.getBorneSuperieur() + 1, BLANC.getBorneInferieur() - 1);

            Button.ENTER.waitForPressAndRelease();

            LCD.drawString("gris fonce", 0, 0);
            while (!Button.ENTER.isPressed()) {
                valeurTestee = readLightValue();
                LCD.drawString("Valeur :" + valeurTestee + "     ", 0, 1);
                LCD.refresh();
            }
            if (GRIS_INTERMEDIAIRE.contient(valeurTestee)) {
                GRIS_INTERMEDIAIRE.setBorneInferieur(valeurTestee + 1);
            }
            GRIS_FONCE = new Range(Robot.NOIR.getBorneSuperieur() + 1, GRIS_INTERMEDIAIRE.getBorneInferieur() - 1);

            Button.ENTER.waitForPressAndRelease();
        } catch (Exception ex) {
            Robot.justSay("Erreur calibrerCouleur");
        }
    }

    /**
     * Calibre la notion de distance à l'obstacle
     * @see SEUIL_OBSTACLE
     */
    public static void calibrerSeuilObstacle() {
        try {

            LCD.drawString("obstacle à ", 0, 0);
            LCD.drawString("Valider : ENTER", 0, 2);
            int valeurTestee = 0;
            while (!Button.ENTER.isPressed()) {
                valeurTestee = UltrasonicSensor.getDistance();
                LCD.drawString("Valeur :" + valeurTestee + "     ", 0, 1);
                LCD.refresh();
            }
            SEUIL_OBSTACLE = valeurTestee;
        } catch (Exception ex) {
            Robot.justSay("Erreur calibrerSeuilObstacle");
        }
    }

    /**
     * Affichage du menu
     */
    public static void afficherMenu() {
        LCD.clearDisplay();
        LCD.drawString("NAVIGUER: ENTER", 0, 0);
        LCD.drawString("CAL. COUL: LEFT", 0, 1);
        LCD.drawString("CAL. OBST: RIGHT", 0, 2);
        LCD.drawString("QUITTER : ESCAPE", 0, 3);
    }

    /**
     * @return la valeur lue par le capteur de lumière
     */
    public static int readLightValue() {
        return Robot.LightSensor.readNormalizedValue();
    }

    /**
     * Rappelle en première ligne qu'escape permet de quitter
     * @param what ce que le robot dit en second ligne si court, écrit sur sept lignes au max.
     */
    public static void justSay(String what) {
        LCD.clearDisplay();
        LCD.drawString("ESC : QUITTER", 0, 4);
        if (what.length() <= 16) {
            LCD.drawString(what, 0, 2);
        } else {
            LCD.drawString(what, 0, 2);
//            try {
//                String[] splitArray = what.split("(?<=\\G.{16})");
//
//                for (int i = 0; i == 6 || i == splitArray.length; i++) {
//                    LCD.drawString(splitArray[0], 0, i + 1);
//                }
//            } catch (PatternSyntaxException ex) {
//                LCD.drawString("Erreur LCD pattern", 0, 2);
//            } catch (Exception ex) {
//                LCD.drawString("Erreur gest. parole", 0, 2);
//            }


        }
        LCD.refresh();
    }
}
